﻿import cv2
import numpy as np
import time
import math
import global_var as glv
import visual_function as vf
from basic_game import basic_game

class baffle(basic_game):
    def __init__(self, running = False):
        ###云台初始化###
        self.PWM_Y = 1700 #云台角度初始化
        basic_game.__init__(self)
        ###标志位###
        self.throughGate = False #已通过该关卡
        self.get_color = False #是否获得目标颜色区块
        ###阈值###
        self.target_color = "blue"
        self.perY = 100
        self.PWM_MaxY = 2300 #跟踪目标体最低角度
        self.minCenter = self.resize_width/2 - self.resize_width/8 #中心区域左边界
        self.maxCenter = self.resize_width/2 + self.resize_width/8 #中心区域右边界
        self.threahold_baffle = self.resize_height - self.resize_height/3 #执行过挡板动作阈值
        self.threahold_change_PWM = self.resize_height - self.resize_height/3 #在云台未到目标值时目标接近需要更高云台的阈值
        ###变量####
        self.lineList = None
        self.center_x = None


    def logic_baffic(self):
        if self.get_color:
            self.get_color = False
            #已足够靠近目标物体
            if self.PWM_Y >= self.PWM_MaxY:             
                if self.lineList["down_y"]> self.threahold_baffle:
                    self.state = "attention"
                    basic_game.execute(self)
                    #############################
                    self.state = "turn_right"
                    basic_game.execute(self)
                    ##########################
                    self.state = "pass_baffle"
                    basic_game.execute(self)
                    self.throughGate = True
                else:
                    self.state = "go_straight"
                    basic_game.execute(self)
                    self.state = "change_PWM"
                    basic_game.execute(self, self.PWM_H_UpY)
                    self.headUp = True
            else:
                if self.lineList["down_y"] >= self.threahold_change_PWM:
                    self.state = "change_PWM"
                    self.PWM_Y += self.perY
                    if self.PWM_Y >= self.PWM_MaxY:
                        self.PWM_Y = self.PWM_MaxY
                    basic_game.execute(self, self.PWM_Y)
                else:
                    self.state = "go_straight"
                    basic_game.execute(self)
                    self.state = "change_PWM"
                    basic_game.execute(self, self.PWM_H_UpY)
                    self.headUp = True

    def run(self, hsvImage, outImage = None):
        if self.headUp:
            self.get_line, self.lines = vf.getLineByColor(hsvImage, 'black',outImage = outImage)
            if outImage is not None:
                cv2.imshow("stepframe",outImage)
                cv2.waitKey(1)
            basic_game.logic_position_up(self)
            return False
        elif self.headDown:
            self.get_line, self.lines = vf.getLineByColor(hsvImage, 'black',outImage = outImage)
            if outImage is not None:
                cv2.imshow("stepframe",outImage)
                cv2.waitKey(1)
            basic_game.logic_position_down(self)
            return False
        else:
            self.get_color, self.lineList, self.center_x = vf.getAreaByColor(hsvImage, self.target_color, outImage)
            if outImage is not None:
                cv2.imshow("stepframe",outImage)
                cv2.waitKey(1)
            baffle.logic_baffic(self)
            if self.throughGate:
                return True
        return False
        